% 数据批量处理程序
close all; clear; clc;

% 获取当前文件所在目录
current_dir = fileparts(mfilename('fullpath'));
default_data_dir = fullfile(current_dir, 'data');

% 让用户输入指定的周期数
fprintf('请输入要保存的指定周期数（例如：10表示保存第10个周期的数据）：\n');
specified_cycle = input('指定周期数: ');

% 验证输入
if isempty(specified_cycle) || ~isnumeric(specified_cycle) || specified_cycle < 1
    fprintf('输入无效，使用默认值：最后一个周期\n');
    specified_cycle = -1; % -1表示使用最后一个周期
end

% 初始化全局变量来存储指定周期的数据
% 结构：{速度, {电流, 角度数据, 扭矩数据}}
specified_cycle_data = containers.Map('KeyType', 'double', 'ValueType', 'any');

% 初始化全局变量来存储峰值数据
% 结构：{速度, {电流, 周期数, 正峰值, 负峰值}}
peak_data_collection = containers.Map('KeyType', 'double', 'ValueType', 'any');

% 让用户选择目标文件夹
fprintf('请选择包含CSV文件的文件夹...\n');
data_dir = uigetdir(default_data_dir, '选择包含CSV文件的文件夹');

% 检查用户是否取消了选择
if data_dir == 0
    fprintf('用户取消了文件夹选择，程序退出\n');
    return;
end

processed_dir = fullfile(current_dir, 'processed');

% 检查选择的文件夹是否存在
if ~exist(data_dir, 'dir')
    error('选择的文件夹不存在: %s', data_dir);
end

% 创建processed文件夹
if ~exist(processed_dir, 'dir')
    mkdir(processed_dir);
    fprintf('已创建processed文件夹\n');
end

% 获取所有csv文件
csv_files = dir(fullfile(data_dir, '*.csv'));
if isempty(csv_files)
    error('选择的文件夹中没有找到csv文件: %s', data_dir);
end

fprintf('选择的文件夹: %s\n', data_dir);
fprintf('找到 %d 个csv文件需要处理\n', length(csv_files));

% 创建进度条
h = waitbar(0, '正在处理数据文件...', 'Name', '数据批量处理进度');

% 批量处理每个文件
for i = 1:length(csv_files)
    try
        close all; % 每次循环开始前关闭所有图窗，清理缓存
        % 更新进度条
        try
            if exist('h', 'var') && ishandle(h)
                waitbar(i/length(csv_files), h, sprintf('处理文件 %d/%d: %s', i, length(csv_files), csv_files(i).name));
            end
        catch
            % 如果进度条有问题，忽略错误继续处理
        end
        
        % 读取CSV文件
        file_path = fullfile(data_dir, csv_files(i).name);
        data = readtable(file_path);
        
        % ========== 从文件名解析三角波参数 ==========
        % 兼容多种文件名格式
        filename = csv_files(i).name;
        fprintf('处理文件: %s\n', filename);
        
        % 提取参数
        target_current = NaN;
        target_amplitude = NaN;
        target_speed = NaN;
        target_cycles = NaN;
        
        % 支持新的文件名格式：电流用下划线表示小数点
        % 例如：triangle_wave_data_0A_30deg_120dps_20cycles.csv (0A)
        %       triangle_wave_data_0_5A_30deg_120dps_20cycles.csv (0.5A)
        %       triangle_wave_data_1A_30deg_120dps_20cycles.csv (1A)
        patterns = {
            'triangle_wave_data_(\d+_?\d*)A_(\d+)deg_(\d+)dps_(\d+)cycles', ...
            'triangle_wave_data_(\d+_?\d*)A_(\d+)deg_(\d+)dps', ...
            'triangle_wave_data_(\d+_?\d*)A_(\d+)deg_(\d+)dps_?(\d*)' ...
        };
        parsed = false;
        for pi = 1:length(patterns)
            tokens = regexp(filename, patterns{pi}, 'tokens');
            if ~isempty(tokens)
                t = tokens{1};
                if length(t) >= 3
                    % 处理电流值：将下划线替换为小数点
                    current_str = strrep(t{1}, '_', '.');
                    target_current = str2double(current_str);
                    target_amplitude = str2double(t{2});
                    target_speed = str2double(t{3});
                    if length(t) >= 4 && ~isempty(t{4})
                        target_cycles = str2double(t{4});
                    end
                    parsed = true;
                    break;
                end
            end
        end
        if parsed
            fprintf('  目标参数: 电流=%.2fA, 角度=%.0f°, 速度=%.0fdps, 周期数=%s\n', ...
                target_current, target_amplitude, target_speed, mat2str(target_cycles));
        else
            fprintf('  警告: 无法从文件名解析参数，使用默认值\n');
            target_amplitude = 30; % 默认30度
            target_speed = 10;     % 默认10度/秒
        end
        
        % 检查数据完整性
        if height(data) < 10
            fprintf('警告: 文件 %s 数据点过少，跳过处理\n', csv_files(i).name);
            continue;
        end
        
        % 检查必要的列是否存在
        required_cols = {'Time_s', 'EncoderAngle_deg', 'Torque_Nm'};
        missing_cols = setdiff(required_cols, data.Properties.VariableNames);
        if ~isempty(missing_cols)
            fprintf('警告: 文件 %s 缺少必要的列: %s，跳过处理\n', csv_files(i).name, strjoin(missing_cols, ', '));
            continue;
        end
        
        % 获取原始列名
        original_vars = data.Properties.VariableNames;
        
        % 提取时间、角度和扭矩数据
        time_data = data.Time_s;
        angle_data = data.EncoderAngle_deg;
        torque_data = data.Torque_Nm;
        
        % 检查数据长度一致性
        if length(time_data) ~= length(angle_data) || length(time_data) ~= length(torque_data)
            fprintf('警告: 文件 %s 数据长度不一致，跳过处理\n', csv_files(i).name);
            continue;
        end
        
        % 检查数据完整性
        if height(data) < 10
            fprintf('警告: 文件 %s 数据点过少，跳过处理\n', csv_files(i).name);
            continue;
        end
        
        % 检查必要的列是否存在
        required_cols = {'Time_s', 'EncoderAngle_deg', 'Torque_Nm'};
        missing_cols = setdiff(required_cols, data.Properties.VariableNames);
        if ~isempty(missing_cols)
            fprintf('警告: 文件 %s 缺少必要的列: %s，跳过处理\n', csv_files(i).name, strjoin(missing_cols, ', '));
            continue;
        end
        
        % 获取原始列名
        original_vars = data.Properties.VariableNames;
        
        % 提取时间、角度和扭矩数据
        time_data = data.Time_s;
        angle_data = data.EncoderAngle_deg;
        torque_data = data.Torque_Nm;
        
        % ========== 数据滤波处理 ==========
        % 1. 异常值检测与去除
        mean_torque = mean(torque_data);
        std_torque = std(torque_data);
        outlier_threshold = 3 * std_torque; % 3倍标准差作为异常值阈值
        outliers = abs(torque_data - mean_torque) > outlier_threshold;
        
        % 用前后点的平均值替换异常值
        torque_cleaned = torque_data;
        for j = find(outliers)'
            if j > 1 && j < length(torque_data)
                torque_cleaned(j) = (torque_data(j-1) + torque_data(j+1)) / 2;
            elseif j == 1
                torque_cleaned(j) = torque_data(j+1);
            elseif j == length(torque_data)
                torque_cleaned(j) = torque_data(j-1);
            end
        end
        
        % 2. 中值滤波（对尖峰噪音最有效）- 加大窗口
        window_size_median = 11; % 从5增加到11
        torque_median_filtered = medfilt1(torque_cleaned, window_size_median);
        
        % 3. 高斯滤波（进一步平滑）- 加大窗口
        window_size_gaussian = 15; % 从5增加到15
        torque_gaussian_filtered = smoothdata(torque_median_filtered, 'gaussian', window_size_gaussian);
        
        % 4. 移动平均滤波（进一步平滑）
        window_size_moving_avg = 9;
        torque_moving_avg = smoothdata(torque_gaussian_filtered, 'movmean', window_size_moving_avg);
        
        % 5. 最终低通滤波
        window_size_final = 7;
        torque_filtered = smoothdata(torque_moving_avg, 'lowess', window_size_final);
        
        % 角度数据也进行更强的滤波
        angle_filtered = smoothdata(angle_data, 'gaussian', 7); % 从3增加到7
        angle_filtered = smoothdata(angle_filtered, 'lowess', 5); % 额外添加低通滤波
        
        % 统计去噪效果
        noise_removed = sum(outliers);
        if noise_removed > 0
            fprintf('  检测到并移除了 %d 个异常值\n', noise_removed);
        end
        fprintf('  滤波参数: 中值滤波窗口=%d, 高斯滤波窗口=%d, 移动平均窗口=%d, 最终低通窗口=%d\n', ...
            window_size_median, window_size_gaussian, window_size_moving_avg, window_size_final);
        
        % ========== 重新计算正负峰值 ==========
        % 改进的周期识别：使用位移数据验证周期划分，并对齐不同周期
        
        % 1. 使用目标幅值来辅助判断周期
        if isnan(target_amplitude)
            target_amplitude = 30; % 默认值
        end
        
        % 2. 计算预期的周期时间
        if ~isnan(target_speed) && target_speed > 0
            expected_cycle_time = 2 * target_amplitude / target_speed; % 一个完整周期的时间
            expected_points_per_cycle = round(expected_cycle_time / mean(diff(time_data)));
        else
            expected_points_per_cycle = 200; % 默认值
        end
        
        fprintf('  预期周期时间: %.2fs, 预期每周期点数: %d\n', ...
            expected_cycle_time, expected_points_per_cycle);
        
        % 3. 使用位移数据识别周期
        % 定义零点容差（角度接近0的范围）
        zero_threshold = max(2, target_amplitude * 0.05); % 5%的幅值作为容差
        
        % 定义幅值容差（角度接近目标幅值的范围）
        amplitude_threshold = target_amplitude * 0.15; % 15%的容差
        
        fprintf('  零点容差: ±%.1f°, 幅值容差: %.1f°~%.1f°\n', ...
            zero_threshold, target_amplitude - amplitude_threshold, target_amplitude + amplitude_threshold);
        
        % 4. 寻找零点（角度接近0的点）
        zero_points = find(abs(angle_filtered) < zero_threshold);
        
        % 5. 过滤掉太近的零点
        if length(zero_points) > 1
            min_interval = max(20, expected_points_per_cycle * 0.3); % 至少30%的预期周期长度
            d = diff(zero_points);
            if isempty(d)
                keep_mask = true(size(zero_points));
            else
                keep_mask = [true; d > min_interval];
            end
            keep_mask = keep_mask(1:length(zero_points));
            zero_points = zero_points(keep_mask);
        end
        
        fprintf('  找到 %d 个零点\n', length(zero_points));
        
        % 6. 识别完整的三角波周期
        % 一个完整周期：从零点 -> 幅值点 -> 零点
        complete_cycles = [];
        
        if length(zero_points) >= 2
            for j = 1:length(zero_points)-1
                start_idx = zero_points(j);
                end_idx = zero_points(j+1);
                
                % 检查这个区间是否足够长
                if end_idx - start_idx > expected_points_per_cycle * 0.5 % 至少50%的预期长度
                    
                    % 在这个区间内寻找幅值点（角度接近目标幅值的点）
                    cycle_angles = angle_filtered(start_idx:end_idx);
                    amplitude_points = find(abs(cycle_angles - target_amplitude) < amplitude_threshold);
                    
                    if ~isempty(amplitude_points)
                        % 找到最接近目标幅值的点
                        [~, best_idx] = min(abs(cycle_angles(amplitude_points) - target_amplitude));
                        amplitude_idx = amplitude_points(best_idx);
                        actual_amplitude = cycle_angles(amplitude_idx);
                        
                        % 验证这是一个完整的三角波周期
                        % 检查幅值点是否在零点和零点之间
                        if amplitude_idx > 1 && amplitude_idx < length(cycle_angles)
                            % 检查前半段是否从0上升到幅值
                            first_half = cycle_angles(1:amplitude_idx);
                            % 检查后半段是否从幅值下降到0
                            second_half = cycle_angles(amplitude_idx:end);
                            
                            % 简单的单调性检查
                            first_monotonic = all(diff(first_half) >= -0.5); % 允许小的噪声
                            second_monotonic = all(diff(second_half) <= 0.5); % 允许小的噪声
                            
                            if first_monotonic && second_monotonic
                                % 记录这个完整周期
                                actual_start_idx = start_idx;
                                actual_amplitude_idx = start_idx + amplitude_idx - 1;
                                actual_end_idx = end_idx;
                                
                                complete_cycles = [complete_cycles; actual_start_idx, actual_amplitude_idx, actual_end_idx];
                                
                                fprintf('  周期 %d: 幅值=%.1f° (目标=%.1f°), 长度=%d点, 时间=%.2fs\n', ...
                                    size(complete_cycles, 1), actual_amplitude, target_amplitude, ...
                                    end_idx - start_idx, time_data(end_idx) - time_data(start_idx));
                            end
                        end
                    end
                end
            end
        end
        
        % 7. 验证周期数量是否符合预期
        if ~isnan(target_cycles) && size(complete_cycles, 1) ~= target_cycles
            fprintf('  警告: 检测到 %d 个周期，但文件名显示应为 %d 个周期\n', ...
                size(complete_cycles, 1), target_cycles);
        end
        
        % 8. 保存所有周期用于峰值检测，但只选择最后5个周期用于误差分析
        total_cycles = size(complete_cycles, 1);
        cycles_to_analyze = min(5, total_cycles); % 误差分析只使用最后5个周期
        
        if total_cycles > 5
            fprintf('  检测到 %d 个周期，峰值检测将使用全部周期，误差分析使用最后5个周期\n', total_cycles);
        else
            fprintf('  检测到 %d 个周期，峰值检测和误差分析都使用全部周期\n', total_cycles);
        end
        
        if total_cycles == 0
            fprintf('  警告: 未找到完整周期，使用简单分割\n');
            % 如果没有找到完整周期，使用简单的零点分割
            zero_crossings = [1; zero_points; length(angle_filtered)];
            total_cycles = min(20, length(zero_crossings) - 1);
            cycles_to_analyze = min(5, total_cycles);
            complete_cycles = zeros(total_cycles, 3);
            for j = 1:total_cycles
                if j <= length(zero_crossings) - 1
                    complete_cycles(j, :) = [zero_crossings(j), 0, zero_crossings(j+1)];
                end
            end
        end
        
        fprintf('  总周期数: %d, 误差分析周期数: %d\n', total_cycles, cycles_to_analyze);
        
        % 每周期找峰值 - 对所有检测到的周期进行峰值检测
        pos_peak_time = nan(total_cycles, 1);
        pos_peak_val = nan(total_cycles, 1);
        neg_peak_time = nan(total_cycles, 1);
        neg_peak_val = nan(total_cycles, 1);
        
        % 存储每个周期的完整数据用于误差分析（只存储最后5个周期）
        cycle_data = cell(cycles_to_analyze, 1);
        
        fprintf('  开始峰值检测，共检测到 %d 个周期\n', total_cycles);
        
        for k = 1:total_cycles
            start_idx = complete_cycles(k, 1);
            end_idx = complete_cycles(k, 3);
            
            % 确保索引在有效范围内
            if start_idx > length(torque_filtered) || end_idx > length(torque_filtered)
                fprintf('  周期 %d 索引超出范围，跳过\n', k);
                continue;
            end
            
            if end_idx >= start_idx && start_idx <= length(torque_filtered) && end_idx <= length(torque_filtered)
                % 提取这个周期的扭矩数据
                cycle_torque = torque_filtered(start_idx:end_idx);
                
                % 找到正峰值和负峰值
                [pmax, imax] = max(cycle_torque);
                [pmin, imin] = min(cycle_torque);
                
                % 存储峰值信息
                pos_peak_val(k) = pmax;
                neg_peak_val(k) = pmin;
                
                % 计算对应的时间
                time_idx_max = start_idx + imax - 1;
                time_idx_min = start_idx + imin - 1;
                
                if time_idx_max <= length(time_data)
                    pos_peak_time(k) = time_data(time_idx_max);
                end
                if time_idx_min <= length(time_data)
                    neg_peak_time(k) = time_data(time_idx_min);
                end
                
                % 调试信息
                fprintf('  周期 %d: 正峰值=%.3f N·m (时间=%.2fs), 负峰值=%.3f N·m (时间=%.2fs)\n', ...
                    k, pmax, pos_peak_time(k), pmin, neg_peak_time(k));
                
                % 只为最后5个周期存储完整数据（用于误差阴影图）
                if k > total_cycles - cycles_to_analyze
                    cycle_idx = k - (total_cycles - cycles_to_analyze);
                    if end_idx <= length(angle_filtered)
                        cycle_data{cycle_idx} = [angle_filtered(start_idx:end_idx), torque_filtered(start_idx:end_idx)];
                    end
                end
                
                % 存储指定周期的数据
                if specified_cycle > 0 && k == specified_cycle
                    if end_idx <= length(angle_filtered)
                        cycle_angles = angle_filtered(start_idx:end_idx);
                        cycle_torques = torque_filtered(start_idx:end_idx);
                        
                        % 存储到全局变量中
                        if ~isKey(specified_cycle_data, target_speed)
                            specified_cycle_data(target_speed) = {};
                        end
                        speed_data = specified_cycle_data(target_speed);
                        speed_data{end+1} = {target_current, cycle_angles, cycle_torques};
                        specified_cycle_data(target_speed) = speed_data;
                        
                        fprintf('  已保存第%d周期数据 (电流=%.1fA, 速度=%ddps)\n', k, target_current, target_speed);
                    end
                elseif specified_cycle == -1 && k == total_cycles
                    % 如果指定周期为-1，保存最后一个周期
                    if end_idx <= length(angle_filtered)
                        cycle_angles = angle_filtered(start_idx:end_idx);
                        cycle_torques = torque_filtered(start_idx:end_idx);
                        
                        % 存储到全局变量中
                        if ~isKey(specified_cycle_data, target_speed)
                            specified_cycle_data(target_speed) = {};
                        end
                        speed_data = specified_cycle_data(target_speed);
                        speed_data{end+1} = {target_current, cycle_angles, cycle_torques};
                        specified_cycle_data(target_speed) = speed_data;
                        
                        fprintf('  已保存最后周期数据 (电流=%.1fA, 速度=%ddps)\n', target_current, target_speed);
                    end
                end
                
                % 收集峰值数据到全局变量
                if ~isKey(peak_data_collection, target_speed)
                    peak_data_collection(target_speed) = {};
                end
                speed_peak_data = peak_data_collection(target_speed);
                speed_peak_data{end+1} = {target_current, k, pmax, pmin};
                peak_data_collection(target_speed) = speed_peak_data;
            else
                fprintf('  周期 %d 索引无效，跳过\n', k);
            end
        end
        
        % ========== 创建合并的数据表 ==========
        % 创建主数据表（时间序列数据）
        main_data = table(time_data, angle_data, torque_data, ...
            angle_filtered, torque_filtered, ...
            'VariableNames', {sprintf('Time_s_%.1fA_%ddps', target_current, target_speed), ...
            sprintf('Angle_deg_%.1fA_%ddps', target_current, target_speed), ...
            sprintf('Torque_Nm_%.1fA_%ddps', target_current, target_speed), ...
            sprintf('Angle_filtered_deg_%.1fA_%ddps', target_current, target_speed), ...
            sprintf('Torque_filtered_Nm_%.1fA_%ddps', target_current, target_speed)});
        
        % 添加峰值信息
        nrows = max(length(pos_peak_time), length(neg_peak_time));
        cycle_numbers_pad = nan(nrows, 1);
        pos_locs_pad = nan(nrows, 1);
        pos_peaks_pad = nan(nrows, 1);
        neg_locs_pad = nan(nrows, 1);
        neg_peaks_pad = nan(nrows, 1);
        
        % 确保不会超出数组边界
        actual_pos_length = min(total_cycles, length(pos_peak_time));
        actual_neg_length = min(total_cycles, length(neg_peak_time));
        
        if actual_pos_length > 0
            cycle_numbers_pad(1:actual_pos_length) = (1:actual_pos_length)';
            pos_locs_pad(1:actual_pos_length) = pos_peak_time(1:actual_pos_length);
            pos_peaks_pad(1:actual_pos_length) = pos_peak_val(1:actual_pos_length);
        end
        if actual_neg_length > 0
            neg_locs_pad(1:actual_neg_length) = neg_peak_time(1:actual_neg_length);
            neg_peaks_pad(1:actual_neg_length) = neg_peak_val(1:actual_neg_length);
        end
        
        % 创建峰值数据表
        peak_data = table(cycle_numbers_pad, pos_locs_pad, pos_peaks_pad, neg_locs_pad, neg_peaks_pad, ...
            'VariableNames', {sprintf('Cycle_Number_%.1fA_%ddps', target_current, target_speed), ...
            sprintf('Positive_Peak_Time_s_%.1fA_%ddps', target_current, target_speed), ...
            sprintf('Positive_Peak_Torque_Nm_%.1fA_%ddps', target_current, target_speed), ...
            sprintf('Negative_Peak_Time_s_%.1fA_%ddps', target_current, target_speed), ...
            sprintf('Negative_Peak_Torque_Nm_%.1fA_%ddps', target_current, target_speed)});
        
        % 创建统计信息表
        stats_data = table(total_cycles, noise_removed, ...
            target_current, target_amplitude, target_speed, target_cycles, ...
            mean(pos_peak_val, 'omitnan'), std(pos_peak_val, 'omitnan'), ...
            mean(neg_peak_val, 'omitnan'), std(neg_peak_val, 'omitnan'), ...
            'VariableNames', {'Total_Cycles', 'Outliers_Removed', ...
            'Target_Current_A', 'Target_Amplitude_deg', 'Target_Speed_dps', 'Target_Cycles', ...
            'Positive_Peak_Mean_Nm', 'Positive_Peak_Std_Nm', ...
            'Negative_Peak_Mean_Nm', 'Negative_Peak_Std_Nm'});
        
        % ========== 保存合并的文件 ==========
        [~, name, ~] = fileparts(csv_files(i).name);
        new_filename = [name, '_processed.csv'];
        new_filepath = fullfile(processed_dir, new_filename);
        
        % 保存主要数据
        writetable(main_data, new_filepath);
        
        % 在同一个文件中追加峰值数据（添加分隔行）
        fid = fopen(new_filepath, 'a');
        fprintf(fid, '\n'); % 空行分隔
        fprintf(fid, '=== 峰值数据 ===\n');
        fclose(fid);
        
        % 写入峰值数据（不包含表头，因为已经在上面添加了说明）
        writetable(peak_data, new_filepath, 'WriteMode', 'append', 'WriteVariableNames', false);
        
        % 在同一个文件中追加统计信息
        fid = fopen(new_filepath, 'a');
        fprintf(fid, '\n'); % 空行分隔
        fprintf(fid, '=== 统计信息 ===\n');
        fclose(fid);
        
        % 写入统计信息（不包含表头，因为已经在上面添加了说明）
        writetable(stats_data, new_filepath, 'WriteMode', 'append', 'WriteVariableNames', false);
        
        % ========== 保存每个周期的单独数据 ==========
        % 创建周期数据文件
        cycle_filename = [name, '_cycles.csv'];
        cycle_filepath = fullfile(processed_dir, cycle_filename);
        
        % 收集所有周期的数据
        all_cycle_data = [];
        max_cycle_length = 0;
        
        % 找到最长的周期长度
        for k = 1:total_cycles
            if k <= size(complete_cycles, 1)
                start_idx = complete_cycles(k, 1);
                end_idx = complete_cycles(k, 3);
                if start_idx <= length(angle_filtered) && end_idx <= length(angle_filtered)
                    cycle_length = end_idx - start_idx + 1;
                    max_cycle_length = max(max_cycle_length, cycle_length);
                end
            end
        end
        
        % 创建列标题
        column_names = {};
        for k = 1:total_cycles
            column_names{end+1} = sprintf('Cycle_%d_%.1fA_%ddps_Angle_deg', k, target_current, target_speed);
            column_names{end+1} = sprintf('Cycle_%d_%.1fA_%ddps_Torque_Nm', k, target_current, target_speed);
        end
        
        % 创建数据矩阵
        cycle_matrix = nan(max_cycle_length, total_cycles * 2);
        
        % 填充数据
        for k = 1:total_cycles
            if k <= size(complete_cycles, 1)
                start_idx = complete_cycles(k, 1);
                end_idx = complete_cycles(k, 3);
                
                if start_idx <= length(angle_filtered) && end_idx <= length(angle_filtered)
                    cycle_angles = angle_filtered(start_idx:end_idx);
                    cycle_torques = torque_filtered(start_idx:end_idx);
                    cycle_length = length(cycle_angles);
                    
                    % 填充角度数据
                    cycle_matrix(1:cycle_length, (k-1)*2 + 1) = cycle_angles;
                    % 填充扭矩数据
                    cycle_matrix(1:cycle_length, (k-1)*2 + 2) = cycle_torques;
                end
            end
        end
        
        % 创建周期数据表
        cycle_data_table = array2table(cycle_matrix, 'VariableNames', column_names);
        
        % 保存周期数据
        writetable(cycle_data_table, cycle_filepath);
        
        % ========== 绘制处理结果对比图 ==========
        fig = figure('Visible','on','Position',[100, 100, 1600, 1000]);
        % ========== 原始数据 vs 滤波后数据 ==========
        subplot(2,4,1);
        plot(time_data, torque_data, 'b-', 'LineWidth', 0.5, 'DisplayName', '原始扭矩');
        hold on;
        plot(time_data, torque_filtered, 'r-', 'LineWidth', 1.5, 'DisplayName', '滤波后扭矩');
        % 标记异常值
        if any(outliers)
            plot(time_data(outliers), torque_data(outliers), 'ko', 'MarkerSize', 8, 'DisplayName', '异常值');
        end
        title('扭矩数据对比'); xlabel('时间 (s)'); ylabel('扭矩 (Nm)');
        legend; grid on;
        
        % 去噪过程可视化
        subplot(2,4,2);
        plot(time_data, torque_data, 'b-', 'LineWidth', 0.5, 'DisplayName', '原始数据');
        hold on;
        plot(time_data, torque_cleaned, 'g-', 'LineWidth', 1, 'DisplayName', '异常值去除后');
        plot(time_data, torque_median_filtered, 'm-', 'LineWidth', 1, 'DisplayName', '中值滤波后');
        plot(time_data, torque_gaussian_filtered, 'c-', 'LineWidth', 1, 'DisplayName', '高斯滤波后');
        plot(time_data, torque_moving_avg, 'y-', 'LineWidth', 1, 'DisplayName', '移动平均后');
        plot(time_data, torque_filtered, 'r-', 'LineWidth', 1.5, 'DisplayName', '最终滤波结果');
        title('去噪过程'); xlabel('时间 (s)'); ylabel('扭矩 (Nm)');
        legend; grid on;
        
        subplot(2,4,3);
        plot(time_data, angle_data, 'b-', 'LineWidth', 0.5, 'DisplayName', '原始角度');
        hold on;
        plot(time_data, angle_filtered, 'r-', 'LineWidth', 1.5, 'DisplayName', '滤波后角度');
        title('角度数据对比'); xlabel('时间 (s)'); ylabel('角度 (度)');
        legend; grid on;
        
        % 滤波后的扭矩-角度关系
        subplot(2,4,4);
        % 使用平缓曲线连接而不是散点图
        plot(angle_filtered, torque_filtered, 'b-', 'LineWidth', 1.5);
        hold on;
        % 标记峰值点 - 根据时间找到对应的角度索引
        pos_peak_angle_idx = zeros(size(pos_peak_time));
        neg_peak_angle_idx = zeros(size(neg_peak_time));
        for k = 1:length(pos_peak_time)
            if ~isnan(pos_peak_time(k))
                [~, pos_peak_angle_idx(k)] = min(abs(time_data - pos_peak_time(k)));
            end
        end
        for k = 1:length(neg_peak_time)
            if ~isnan(neg_peak_time(k))
                [~, neg_peak_angle_idx(k)] = min(abs(time_data - neg_peak_time(k)));
            end
        end
        % 确保索引在有效范围内
        pos_peak_angle_idx = pos_peak_angle_idx(pos_peak_angle_idx > 0 & pos_peak_angle_idx <= length(angle_filtered));
        neg_peak_angle_idx = neg_peak_angle_idx(neg_peak_angle_idx > 0 & neg_peak_angle_idx <= length(angle_filtered));
        
        if ~isempty(pos_peak_angle_idx) && ~isempty(pos_peak_val)
            num_pos_peaks = min(length(pos_peak_angle_idx), length(pos_peak_val));
            if num_pos_peaks > 0
                plot(angle_filtered(pos_peak_angle_idx(1:num_pos_peaks)), pos_peak_val(1:num_pos_peaks), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
            end
        end
        if ~isempty(neg_peak_angle_idx) && ~isempty(neg_peak_val)
            num_neg_peaks = min(length(neg_peak_angle_idx), length(neg_peak_val));
            if num_neg_peaks > 0
                plot(angle_filtered(neg_peak_angle_idx(1:num_neg_peaks)), neg_peak_val(1:num_neg_peaks), 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g');
            end
        end
        xlabel('角度 (度)');
        ylabel('扭矩 (N·m)');
        title('力位移关系图');
        grid on;
        legend('扭矩曲线', '正峰值', '负峰值', 'Location', 'best');
        
        % 频谱分析
        subplot(2,4,6);
        if length(torque_filtered) > 1
            % 计算频谱
            Fs = 1/mean(diff(time_data)); % 采样频率
            L = length(torque_filtered);
            Y = fft(torque_filtered - mean(torque_filtered));
            P2 = abs(Y/L);
            
            % 确保索引是整数
            n = floor(L/2);
            P1 = P2(1:n+1);
            P1(2:end-1) = 2*P1(2:end-1);
            freq = Fs*(0:n)/L;
            
            plot(freq, P1, 'b-', 'LineWidth', 1.5);
            xlabel('频率 (Hz)');
            ylabel('功率谱密度');
            title('扭矩信号频谱分析');
            grid on;
            xlim([0, 50]); % 限制到50Hz
        else
            text(0.5, 0.5, '数据不足，无法计算频谱', 'HorizontalAlignment', 'center', 'Units', 'normalized');
            title('扭矩信号频谱分析');
        end
        
        % 峰值对比 - 绘制所有检测到的周期的峰值
        subplot(2,4,7);
        cycle_numbers = 1:total_cycles;
        plot(cycle_numbers, pos_peak_val, 'r.-', 'LineWidth', 2, 'MarkerSize', 15, 'DisplayName', '正峰值');
        hold on;
        plot(cycle_numbers, neg_peak_val, 'b.-', 'LineWidth', 2, 'MarkerSize', 15, 'DisplayName', '负峰值');
        title(sprintf('重新计算的峰值 (共%d个周期)', total_cycles)); xlabel('周期数'); ylabel('扭矩 (Nm)');
        legend; grid on;
        xlim([1, total_cycles]);
        
        % 统计信息
        subplot(2,4,8);
        
        % 安全地计算统计信息，避免索引问题
        if length(pos_peak_val) == 1 && ~isnan(pos_peak_val(1))
            pos_peak_max = pos_peak_val(1);
            pos_peak_min = pos_peak_val(1);
        else
            pos_peak_max = max(pos_peak_val, [], 'omitnan');
            pos_peak_min = min(pos_peak_val, [], 'omitnan');
        end
        
        if length(neg_peak_val) == 1 && ~isnan(neg_peak_val(1))
            neg_peak_max = neg_peak_val(1);
            neg_peak_min = neg_peak_val(1);
        else
            neg_peak_max = max(neg_peak_val, [], 'omitnan');
            neg_peak_min = min(neg_peak_val, [], 'omitnan');
        end
        
        stats_text = sprintf(['统计信息:\n\n' ...
            '目标参数:\n' ...
            '  电流: %.1f A\n' ...
            '  角度: %.0f 度\n' ...
            '  速度: %.0f 度/秒\n' ...
            '  周期数: %s\n\n' ...
            '正峰值统计:\n' ...
            '  平均值: %.3f N·m\n' ...
            '  标准差: %.3f N·m\n' ...
            '  最大值: %.3f N·m\n' ...
            '  最小值: %.3f N·m\n\n' ...
            '负峰值统计:\n' ...
            '  平均值: %.3f N·m\n' ...
            '  标准差: %.3f N·m\n' ...
            '  最大值: %.3f N·m\n' ...
            '  最小值: %.3f N·m\n\n' ...
            '总周期数: %d\n' ...
            '误差分析周期数: %d\n' ...
            '数据点数: %d\n' ...
            '异常值数: %d'], ...
            target_current, target_amplitude, target_speed, ...
            mat2str(target_cycles), ...
            mean(pos_peak_val, 'omitnan'), std(pos_peak_val, 'omitnan'), ...
            pos_peak_max, pos_peak_min, ...
            mean(neg_peak_val, 'omitnan'), std(neg_peak_val, 'omitnan'), ...
            neg_peak_max, neg_peak_min, ...
            total_cycles, cycles_to_analyze, length(torque_filtered), noise_removed);
        text(0.1, 0.5, stats_text, 'Units', 'normalized', 'FontSize', 9);
        axis off;
        
        % ========== 位移-扭矩误差阴影图（最后5个完整周期） ==========
        subplot(2,4,5);
        if cycles_to_analyze > 0 && ~isempty(cycle_data)
            % 只使用最后5个完整周期的数据
            fprintf('  准备绘制最后%d个完整周期的误差阴影图\n', cycles_to_analyze);
            
            % 定义标准化的角度序列（从0到目标幅值再到0）
            num_standard_points = 200; % 标准化的点数
            % 创建完整的三角波角度序列：0 -> 幅值 -> 0
            half_points = num_standard_points / 2;
            standard_angles = [linspace(0, target_amplitude, half_points), ...
                              linspace(target_amplitude, 0, half_points)];
            
            % 确保角度序列正确（调试用）
            fprintf('  标准角度序列范围: %.1f° 到 %.1f° (共%d个点)\n', ...
                min(standard_angles), max(standard_angles), length(standard_angles));
            
            % 存储每个周期对齐后的扭矩数据
            aligned_torque_cycles = zeros(cycles_to_analyze, length(standard_angles));
            valid_cycles_count = 0;
            
            % 对每个周期进行插值对齐
            for k = 1:cycles_to_analyze
                if ~isempty(cycle_data{k}) && size(cycle_data{k}, 1) > 10
                    cycle_angles = cycle_data{k}(:, 1);  % 角度数据
                    cycle_torques = cycle_data{k}(:, 2); % 扭矩数据
                    
                    % 确保角度数据是单调的（从0开始到幅值再到0）
                    if length(cycle_angles) > 1
                        try
                            % 调试：检查原始扭矩值的正负
                            fprintf('  周期 %d 原始扭矩范围: %.3f 到 %.3f N·m\n', k, min(cycle_torques), max(cycle_torques));
                            
                            % 预处理角度数据，确保单调性
                            % 1. 去除重复的角度值
                            [unique_angles, unique_indices] = unique(cycle_angles, 'stable');
                            unique_torques = cycle_torques(unique_indices);
                            
                            % 2. 确保角度序列是单调的（从0到幅值再到0）
                            % 找到幅值点（最接近目标幅值的点）
                            [~, max_idx] = min(abs(unique_angles - target_amplitude));
                            
                            % 分割为上升段和下降段
                            if max_idx > 1 && max_idx < length(unique_angles)
                                % 上升段：从开始到幅值点
                                rising_angles = unique_angles(1:max_idx);
                                rising_torques = unique_torques(1:max_idx);
                                
                                % 下降段：从幅值点到结束
                                falling_angles = unique_angles(max_idx:end);
                                falling_torques = unique_torques(max_idx:end);
                                
                                % 确保上升段是单调递增的
                                [rising_angles_sorted, rising_sort_idx] = sort(rising_angles);
                                rising_torques_sorted = rising_torques(rising_sort_idx);
                                
                                % 确保下降段是单调递减的
                                [falling_angles_sorted, falling_sort_idx] = sort(falling_angles, 'descend');
                                falling_torques_sorted = falling_torques(falling_sort_idx);
                                
                                % 合并处理后的数据
                                processed_angles = [rising_angles_sorted; falling_angles_sorted(2:end)];
                                processed_torques = [rising_torques_sorted; falling_torques_sorted(2:end)];
                                
                                % 调试：检查处理后扭矩值的正负
                                fprintf('  周期 %d 处理后扭矩范围: %.3f 到 %.3f N·m\n', k, min(processed_torques), max(processed_torques));
                                
                                % 使用插值将数据对齐到标准角度位置
                                aligned_torque = interp1(processed_angles, processed_torques, standard_angles, 'linear', 'extrap');
                                
                                % 调试：检查插值后扭矩值的正负
                                fprintf('  周期 %d 插值后扭矩范围: %.3f 到 %.3f N·m\n', k, min(aligned_torque), max(aligned_torque));
                                
                                aligned_torque_cycles(k, :) = aligned_torque;
                                valid_cycles_count = valid_cycles_count + 1;
                                
                                fprintf('  周期 %d 对齐完成: %d点 -> %d点 (处理后: %d点)\n', k, length(cycle_angles), length(standard_angles), length(processed_angles));
                            else
                                fprintf('  周期 %d 幅值点位置异常，跳过\n', k);
                                aligned_torque_cycles(k, :) = NaN;
                            end
                        catch ME
                            fprintf('  周期 %d 对齐失败: %s\n', k, ME.message);
                            aligned_torque_cycles(k, :) = NaN;
                        end
                    else
                        fprintf('  周期 %d 数据点不足，跳过\n', k);
                        aligned_torque_cycles(k, :) = NaN;
                    end
                else
                    fprintf('  周期 %d 数据为空或不足，跳过\n', k);
                    aligned_torque_cycles(k, :) = NaN;
                end
            end
            
            % 只使用有效的周期数据
            valid_cycles = ~all(isnan(aligned_torque_cycles), 2);
            if sum(valid_cycles) > 0
                valid_torque_data = aligned_torque_cycles(valid_cycles, :);
                
                % 计算并绘制平均曲线和误差带
                mean_torque = mean(valid_torque_data, 1, 'omitnan');
                std_torque = std(valid_torque_data, 0, 1, 'omitnan');
                
                % 调试信息
                fprintf('  有效周期数: %d\n', sum(valid_cycles));
                fprintf('  平均扭矩范围: %.3f 到 %.3f N·m\n', min(mean_torque), max(mean_torque));
                fprintf('  标准差范围: %.3f 到 %.3f N·m\n', min(std_torque), max(std_torque));
                
                % 绘制误差带
                upper_bound = mean_torque + std_torque;
                lower_bound = mean_torque - std_torque;
                
                % 使用fill绘制误差带
                x_fill = [standard_angles, fliplr(standard_angles)];
                y_fill = [upper_bound, fliplr(lower_bound)];
                
                % 确保没有NaN值
                valid_fill = ~isnan(x_fill) & ~isnan(y_fill);
                x_fill_valid = x_fill(valid_fill);
                y_fill_valid = y_fill(valid_fill);
                
                if length(x_fill_valid) > 2
                    fill(x_fill_valid, y_fill_valid, 'b', 'FaceAlpha', 0.2, 'EdgeColor', 'none', 'DisplayName', '±1σ误差带');
                    hold on;
                    
                    % 绘制平均曲线
                    plot(standard_angles, mean_torque, 'b-', 'LineWidth', 2, 'DisplayName', '平均扭矩');
                    
                    xlabel('角度 (度)');
                    ylabel('扭矩 (N·m)');
                    title(sprintf('力位移误差阴影图 (最后%d个完整周期)', sum(valid_cycles)));
                    grid on;
                    legend('Location', 'best');
                    
                    fprintf('  成功绘制了 %d 个完整周期的误差阴影图\n', sum(valid_cycles));
                else
                    fprintf('  警告: 有效填充数据不足，无法绘制误差带\n');
                    % 只绘制平均曲线
                    plot(standard_angles, mean_torque, 'b-', 'LineWidth', 2, 'DisplayName', '平均扭矩');
                    xlabel('角度 (度)');
                    ylabel('扭矩 (N·m)');
                    title(sprintf('力位移关系图 (最后%d个完整周期)', sum(valid_cycles)));
                    grid on;
                    legend('Location', 'best');
                end
            else
                text(0.5, 0.5, '没有有效的周期数据', 'HorizontalAlignment', 'center', 'Units', 'normalized');
                title('力位移误差阴影图');
            end
        else
            text(0.5, 0.5, '没有周期数据', 'HorizontalAlignment', 'center', 'Units', 'normalized');
            title('力位移误差阴影图');
        end
        
        % 保存图像
        [~, img_name, ~] = fileparts(new_filename);
        img_path = fullfile(processed_dir, [img_name, '.png']);
        saveas(fig, img_path);
        pause(1); % 展示1秒
        close(fig); % 自动关闭当前图窗
        
        fprintf('已处理: %s -> %s (总周期%d个，误差分析%d个周期，移除%d个异常值)\n', ...
            csv_files(i).name, new_filename, total_cycles, cycles_to_analyze, noise_removed);
        
    catch ME
        close all; % 异常时也关闭所有图窗，防止残留
        fprintf('处理文件 %s 时发生错误: %s\n', csv_files(i).name, ME.message);
        
        % 添加详细的调试信息
        if contains(ME.message, '索引超过数组元素的数量')
            fprintf('调试信息:\n');
            fprintf('  文件索引: %d\n', i);
            fprintf('  总文件数: %d\n', length(csv_files));
            
            % 检查关键变量的长度
            try
                if exist('time_data', 'var')
                    fprintf('  时间数据长度: %d\n', length(time_data));
                end
                if exist('angle_filtered', 'var')
                    fprintf('  角度数据长度: %d\n', length(angle_filtered));
                end
                if exist('torque_filtered', 'var')
                    fprintf('  扭矩数据长度: %d\n', length(torque_filtered));
                end
                if exist('pos_peak_time', 'var')
                    fprintf('  正峰值时间长度: %d\n', length(pos_peak_time));
                end
                if exist('neg_peak_time', 'var')
                    fprintf('  负峰值时间长度: %d\n', length(neg_peak_time));
                end
                if exist('cycles_to_analyze', 'var')
                    fprintf('  分析周期数: %d\n', cycles_to_analyze);
                end
            catch debug_err
                fprintf('  无法获取调试信息: %s\n', debug_err.message);
            end
        end
        
        % 显示错误堆栈
        if ~isempty(ME.stack)
            fprintf('错误位置: %s (第%d行)\n', ME.stack(1).name, ME.stack(1).line);
        end
        
        % 更新进度条（如果还存在）
        try
            if exist('h', 'var') && ishandle(h)
                waitbar(i/length(csv_files), h, sprintf('处理文件 %d/%d: %s (错误)', i, length(csv_files), csv_files(i).name));
            end
        catch
            % 如果进度条有问题，忽略错误继续处理
        end
    end
end

% 安全地关闭进度条
try
    if exist('h', 'var') && ishandle(h)
        close(h);
    end
catch
    % 如果关闭进度条时出错，忽略错误
end

fprintf('\n===== 批量处理完成 =====\n');
fprintf('处理后的文件保存在: %s\n', processed_dir);
fprintf('共处理了 %d 个文件\n', length(csv_files));

% ========== 保存指定周期的数据 ==========
if ~isempty(specified_cycle_data)
    fprintf('\n===== 保存指定周期数据 =====\n');
    
    % 遍历每个速度
    speed_keys = keys(specified_cycle_data);
    for i = 1:length(speed_keys)
        speed = speed_keys{i};
        speed_data = specified_cycle_data(speed);
        
        if ~isempty(speed_data)
            % 找到最长的数据长度
            max_length = 0;
            for j = 1:length(speed_data)
                current_data = speed_data{j};
                if length(current_data) >= 3
                    max_length = max(max_length, length(current_data{2})); % 角度数据长度
                end
            end
            
            if max_length > 0
                % 创建列标题
                column_names = {};
                for j = 1:length(speed_data)
                    current_data = speed_data{j};
                    if length(current_data) >= 3
                        current = current_data{1};
                        column_names{end+1} = sprintf('%.1fA_Angle_deg', current);
                        column_names{end+1} = sprintf('%.1fA_Torque_Nm', current);
                    end
                end
                
                % 创建数据矩阵
                data_matrix = nan(max_length, length(column_names));
                
                % 填充数据
                col_idx = 1;
                for j = 1:length(speed_data)
                    current_data = speed_data{j};
                    if length(current_data) >= 3
                        current = current_data{1};
                        angles = current_data{2};
                        torques = current_data{3};
                        data_length = length(angles);
                        
                        % 填充角度数据
                        data_matrix(1:data_length, col_idx) = angles;
                        col_idx = col_idx + 1;
                        
                        % 填充扭矩数据
                        data_matrix(1:data_length, col_idx) = torques;
                        col_idx = col_idx + 1;
                    end
                end
                
                % 去除每列末尾的NaN值
                % 找到所有列中最后一个有效数据的最大行数
                max_valid_rows = 0;
                for col = 1:size(data_matrix, 2)
                    col_data = data_matrix(:, col);
                    last_valid_idx = find(~isnan(col_data), 1, 'last');
                    if ~isempty(last_valid_idx)
                        max_valid_rows = max(max_valid_rows, last_valid_idx);
                    end
                end
                
                % 只保留到最大有效行数
                if max_valid_rows > 0
                    data_matrix = data_matrix(1:max_valid_rows, :);
                end
                
                % 创建数据表
                cycle_data_table = array2table(data_matrix, 'VariableNames', column_names);
                
                % 保存文件
                if specified_cycle > 0
                    filename = sprintf('specified_cycle_%d_%ddps_data.csv', specified_cycle, speed);
                else
                    filename = sprintf('last_cycle_%ddps_data.csv', speed);
                end
                filepath = fullfile(processed_dir, filename);
                writetable(cycle_data_table, filepath);
                
                fprintf('已保存 %ddps 速度下的指定周期数据: %s\n', speed, filename);
            end
        end
    end
end

% ========== 保存峰值数据 ==========
if ~isempty(peak_data_collection)
    fprintf('\n===== 保存峰值数据 =====\n');
    
    % 遍历每个速度
    speed_keys = keys(peak_data_collection);
    for i = 1:length(speed_keys)
        speed = speed_keys{i};
        speed_peak_data = peak_data_collection(speed);
        
        if ~isempty(speed_peak_data)
            % 创建列标题
            column_names = {'Current_A', 'Cycle_Number', 'Positive_Peak_Nm', 'Negative_Peak_Nm'};
            
            % 创建数据矩阵
            num_peaks = length(speed_peak_data);
            peak_matrix = zeros(num_peaks, 4);
            
            % 填充数据
            for j = 1:num_peaks
                peak_info = speed_peak_data{j};
                if length(peak_info) >= 4
                    peak_matrix(j, 1) = peak_info{1}; % 电流
                    peak_matrix(j, 2) = peak_info{2}; % 周期数
                    peak_matrix(j, 3) = peak_info{3}; % 正峰值
                    peak_matrix(j, 4) = peak_info{4}; % 负峰值
                end
            end
            
            % 创建数据表
            peak_data_table = array2table(peak_matrix, 'VariableNames', column_names);
            
            % 保存文件
            filename = sprintf('peak_data_%ddps.csv', speed);
            filepath = fullfile(processed_dir, filename);
            writetable(peak_data_table, filepath);
            
            fprintf('已保存 %ddps 速度下的峰值数据: %s\n', speed, filename);
        end
    end
end
